/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, LABUST, UNIZG-FER
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the LABUST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
#ifndef USBLSENSOR_HPP_
#define USBLSENSOR_HPP_
#include "osgOceanScene.h"
#include "UWSimUtils.h"

#include <osg/Node>
#include <osg/Matrix>
#include <osg/Group>
#include <osg/Vec3d>

#include <boost/random/mersenne_twister.hpp>

#include <osg/NodeVisitor>
#include <osg/Node>

class USBLSensor {

public:
	std::string name;

	/** Constructor
	 * @param name the name of the pressure sensor
	 * @param parent the node of the scene graph that holds the sensor
	 * @param rMl the sensor measures are given with respect to the root (r). Use rMl to transform them to another frame ('l' is the new frame, typically the localized world)
	 * @param std the standard deviation on the sensor measures
	 */
	USBLSensor(osgOceanScene *oscene, std::string sensor_name, osg::Node *parent, osg::Matrixd rMl, double std, const std::map<int,std::string>& transponders):
		name(sensor_name),
		oscene_(oscene),
		parent_(parent),
		rMl_(rMl),
		std_(std),
		transponders(transponders),
		nextId()
	{
		node_=new osg::Node();
		parent->asGroup()->addChild(node_);
	}

	/**
	 * \todo Assemble a list of nodes to avoid using the node visitor in each iteration.
	 */
	std::pair<int,osg::Vec3d> getMeasurement();

	double getStandardDeviation() {return std_;}

	~USBLSensor() {}

private:
	osg::ref_ptr<osgOceanScene> oscene_;
	osg::ref_ptr<osg::Node> parent_;
	osg::Matrixd rMl_;
	double std_;
	osg::ref_ptr<osg::Node> node_;
	findNodeVisitor findNode;
	std::map<int,std::string> transponders;
	int nextId;

	boost::mt19937 rng_; ///< Boost random number generator
};

/* USBLSENSOR_HPP */
#endif
